#include "pinmap.h"

bool pin_in_pinmap(PinName pin, const PinMap *map)
{
    if (pin != NC)
    {
        while (map->pin != NC)
        {
            if (map->pin == pin)
                return true;
            map++;
        }
    }
    return false;
}

void pin_function(PinName pin, int function)
{
    uint32_t mode = STM_PIN_FUNCTION(function);
    uint32_t afnum = STM_PIN_AFNUM(function);
    uint8_t pupd = STM_PIN_PUPD(function);
    uint32_t port = CH_PORT(pin);
    uint32_t p = CH_PIN(pin);

    if (pin == (PinName)NC)
        Error_Handler();

    GPIO_TypeDef *gpioPort = set_GPIO_Port_Clock(port);
    if (gpioPort == 0)
        Error_Handler();

    uint8_t IOMode;
    switch (mode)
    {
    case STM_MODE_INPUT:
    {
        switch (pupd)
        {
        case GPIO_PULLUP:
            IOMode = GPIO_Mode_IPU;
            break;
        case GPIO_PULLDOWN:
            IOMode = GPIO_Mode_IPD;
            break;
        case GPIO_NOPULL:
        default:
            IOMode = GPIO_Mode_IN_FLOATING;
            break;
        }
    }
    break;
    case STM_MODE_OUTPUT_PP:
        IOMode = GPIO_Mode_Out_PP;
        break;
    case STM_MODE_OUTPUT_OD:
        IOMode = GPIO_Mode_Out_OD;
        break;
    case STM_MODE_AF_PP:
        IOMode = GPIO_Mode_AF_PP;
        break;
    case STM_MODE_AF_OD:
        IOMode = GPIO_Mode_AF_OD;
        break;
    case STM_MODE_ANALOG:
    case STM_MODE_ANALOG_ADC_CONTROL:
        IOMode = GPIO_Mode_AIN;
        break;
    default:
        IOMode = GPIO_Mode_Out_PP;
        break;
    }
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = get_GPIO_Pin(p);
    GPIO_InitStructure.GPIO_Mode = IOMode;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(gpioPort, &GPIO_InitStructure);
}

void pinmap_pinout(PinName pin, const PinMap *map)
{
    if (pin == NC)
    {
        return;
    }

    while (map->pin != NC)
    {
        if (map->pin == pin)
        {
            pin_function(pin, map->function);
            return;
        }
        map++;
    }
    Error_Handler();
}

void *pinmap_find_peripheral(PinName pin, const PinMap *map)
{
    while (map->pin != NC)
    {
        if (map->pin == pin)
            return map->peripheral;

        map++;
    }
    return NP;
}

void *pinmap_peripheral(PinName pin, const PinMap *map)
{
    void *peripheral = NP;

    if (pin != (PinName)NC)
    {
        peripheral = pinmap_find_peripheral(pin, map);
    }
    return peripheral;
}

PinName pinmap_find_pin(void *peripheral, const PinMap *map)
{
    while (map->peripheral != NP)
    {
        if (map->peripheral == peripheral)
            return map->pin;

        map++;
    }
    return NC;
}

PinName pinmap_pin(void *peripheral, const PinMap *map)
{
    PinName pin = NC;

    if (peripheral != NP)
    {
        pin = pinmap_find_pin(peripheral, map);
    }
    return pin;
}

uint32_t pinmap_find_function(PinName pin, const PinMap *map)
{
    while (map->pin != NC)
    {
        if (map->pin == pin)
            return map->function;

        map++;
    }
    return (uint32_t)NC;
}

uint32_t pinmap_function(PinName pin, const PinMap *map)
{
    uint32_t function = (uint32_t)NC;

    if (pin != (PinName)NC)
    {
        function = pinmap_find_function(pin, map);
    }
    return function;
}

void *pinmap_merge_peripheral(void *a, void *b)
{
    if (a == b)
    {
        return a;
    }
    if (a == NP)
    {
        return b;
    }
    if (b == NP)
    {
        return a;
    }
    return NP;
}